#include "pch.h"
#include "CommonMath.h"

Eigen::Vector3f CMathFunctions::translatePostion(Eigen::Vector3f vParticlePos, Eigen::Matrix4f vTransferMatrix)
{
	Eigen::Vector4f vector(vParticlePos[0], vParticlePos[1], vParticlePos[2], 1.0f);
	Eigen::Vector4f result = vTransferMatrix * vector;
	Eigen::Vector3f newPos(result[0], result[1], result[2]);
	return newPos;
}

void CMathFunctions::polarDecompositionStable(Eigen::Matrix3d& M, double tolerance, Eigen::Matrix3d& R)
{
	Eigen::Matrix3d Mt = M.transpose();
	double Mone = oneNorm(M);
	double Minf = infNorm(M);
	double Eone;
	Eigen::Matrix3d MadjTt, Et;
	do
	{
		MadjTt.row(0) = Mt.row(1).cross(Mt.row(2));
		MadjTt.row(1) = Mt.row(2).cross(Mt.row(0));
		MadjTt.row(2) = Mt.row(0).cross(Mt.row(1));

		double det = Mt(0, 0) * MadjTt(0, 0) + Mt(0, 1) * MadjTt(0, 1) + Mt(0, 2) * MadjTt(0, 2);

		if (fabs(det) < 1.0e-12)
		{
			Eigen::Vector3d len;
			unsigned int index = 0xffffffff;
			for (unsigned int i = 0; i < 3; i++)
			{
				len[i] = MadjTt.row(i).squaredNorm();
				if (len[i] > 1.0e-12)
				{
					// index of valid cross product
					// => is also the index of the vector in Mt that must be exchanged
					index = i;
					break;
				}
			}
			if (index == 0xffffffff)
			{
				R.setIdentity();
				return;
			}
			else
			{
				Mt.row(index) = Mt.row((index + 1) % 3).cross(Mt.row((index + 2) % 3));
				MadjTt.row((index + 1) % 3) = Mt.row((index + 2) % 3).cross(Mt.row(index));
				MadjTt.row((index + 2) % 3) = Mt.row(index).cross(Mt.row((index + 1) % 3));
				Eigen::Matrix3d M2 = Mt.transpose();
				Mone = oneNorm(M2);
				Minf = infNorm(M2);
				det = Mt(0, 0) * MadjTt(0, 0) + Mt(0, 1) * MadjTt(0, 1) + Mt(0, 2) * MadjTt(0, 2);
			}
		}

		const double MadjTone = oneNorm(MadjTt);
		const double MadjTinf = infNorm(MadjTt);

		const double gamma = sqrt(sqrt((MadjTone*MadjTinf) / (Mone*Minf)) / fabs(det));

		const double g1 = gamma * static_cast<double>(0.5);
		const double g2 = static_cast<double>(0.5) / (gamma*det);

		for (unsigned char i = 0; i < 3; i++)
		{
			for (unsigned char j = 0; j < 3; j++)
			{
				Et(i, j) = Mt(i, j);
				Mt(i, j) = g1 * Mt(i, j) + g2 * MadjTt(i, j);
				Et(i, j) -= Mt(i, j);
			}
		}

		Eone = oneNorm(Et);

		Mone = oneNorm(Mt);
		Minf = infNorm(Mt);
	} while (Eone > Mone * tolerance);

	// Q = Mt^T 
	R = Mt.transpose();
}

double CMathFunctions::oneNorm(Eigen::Matrix3d& A)
{
	const double sum1 = fabs(A(0, 0)) + fabs(A(1, 0)) + fabs(A(2, 0));
	const double sum2 = fabs(A(0, 1)) + fabs(A(1, 1)) + fabs(A(2, 1));
	const double sum3 = fabs(A(0, 2)) + fabs(A(1, 2)) + fabs(A(2, 2));
	double maxSum = sum1;
	if (sum2 > maxSum)
		maxSum = sum2;
	if (sum3 > maxSum)
		maxSum = sum3;
	return maxSum;
}

double CMathFunctions::infNorm(Eigen::Matrix3d& A)
{
	const double sum1 = fabs(A(0, 0)) + fabs(A(0, 1)) + fabs(A(0, 2));
	const double sum2 = fabs(A(1, 0)) + fabs(A(1, 1)) + fabs(A(1, 2));
	const double sum3 = fabs(A(2, 0)) + fabs(A(2, 1)) + fabs(A(2, 2));
	double maxSum = sum1;
	if (sum2 > maxSum)
		maxSum = sum2;
	if (sum3 > maxSum)
		maxSum = sum3;
	return maxSum;
}

void CMathFunctions::semiImplicitEuler(double h, double mass, Eigen::Vector3d& position, Eigen::Vector3d& velocity, const Eigen::Vector3d& acceleration)
{
	if (mass != 0.0)
	{
		velocity += acceleration * h;
		position += velocity * h;
	}
}

void CMathFunctions::velocityUpdateFirstOrder(double h, double mass, const Eigen::Vector3d& position, const Eigen::Vector3d& oldPosition, Eigen::Vector3d& velocity)
{
	if (mass != 0.0)
		velocity = (1.0 / h) * (position - oldPosition);
}

void CMathFunctions::velocityDamping(double dampFactor, Eigen::Vector3d& velocity)
{
	velocity = velocity * (1.0 - dampFactor);
}